#!/usr/bin/env python
# -*-coding:utf-8 -*-

__all__ = ['Time', 'encode', 'decode', 'Robot', 'getState', 'release', 'initPos', 
           'getPos', 'setVel', 'getVel', 'schedule', 
           'reportWaypoint', 'update', 'step', 'run']

from vector import Vector
from message import Message
from random import random


class Robot(object):
    # when a robot finished a task, it is not necessarily in state STATE_IDLE
    # we need to release it manually
    STATE_IDLE         = 0
    STATE_GOING_TO_DST = 1 # going to destination node, specified by task
    STATE_PAUSE        = 2
    STATE_REACH_DST    = 3

    def __init__(self, world, _id=None, waypoint=None):
        """
        创建一个机器人对象。
        :param world: 仿真的世界。
        :param _id: 机器人的id。
        :param waypoint: 机器人初始时所处的节点。
        """
        super().__init__()

        self._id = _id
        self._world = world
        self._timer = world.getTimer()
        self._map = world.getMap()
        self._robot_pool = world.getRobotPool()
        self._obstacle_pool = world.getObstaclePool()
        self._comm_channel = world.getCommunicationChannel()
        self._state = Robot.STATE_IDLE
        if waypoint is None:
            self._last_waypoint = None
            self._pos = None
        else:
            self._last_waypoint = waypoint
            self._pos = self._map.getNodeCoord(waypoint)
            self.reportWaypoint(waypoint, 0.0)
        self._orientation = 0.0 # angle of the robot's head and the x-axis
        self._vel = Vector(0, 0)
        self._angular_vel = 0.0
        self._path = None
        self._speed = None
        self._last_waypoint_index = None
        self._detection_range = 1.0 # obstacle detection range
        self._detection_angle = 45 # obstacle detection angle

        # TCP/IP connection
        #self._conn = Client()
        #self._time = Time()

    def getState(self):
        """
        获取机器人的状态。
        :return: 机器人的状态。可能的状态为：STATE_IDLE空闲，STATE_GOING_TO_DST朝目的地移动，STATE_REACH_DST到达目的地。
                 当一个机器人完成任务之后，它并不自动地变回空闲状态，需要我们手动设置（通过release函数）。
        """
        return self._state

    def release(self):
        """
        从当前任务中释放机器人，使得它回归到空闲状态。
        """
        if self._state == STATE_REACH_DST:
            self._state = STATE_IDLE
            
    def getMaxSpeed(self):
        """
        FixMe: The max speed should be loaded from configuration file.
        """
        return 1.0

    def setPos(self, pos):
        """
        设置机器人的位置坐标。
        :param pos: 初始位置。用坐标表示。pos需为为Vector对象。
        """
        self._pos = pos

    def getPos(self):
        """
        获取机器人的位置。
        :return: 机器人的位置，用Vector对象表示。
        """
        return self._pos
        
    def getLastWaypoint(self):
        return self._last_waypoint

    def setVel(self, x, y):
        """
        设置机器人的速度。
        :param x: 设置的速度的x分量。
        :param y: 设置的速度的y分量。
        """
        self._vel = Vector(x, y)

    def getVel(self):
        """
        获取机器人的速度。
        :return: 机器人的速度（Vector对象）。
        """
        return self._vel
        
    def getID(self):
        """
        获取机器人的ID。机器人的id即为它在机器人列表中的索引。
        :return: 机器人的ID。
        """
        return self._id
        
    def getDetectionRange(self):
        return self._detection_range
        
    def getDetectionAngle(self):
        return self._detection_angle

    def _schedule(self, path, speed):
        """
        调度机器人。
        :param path: 调度机器人按照该路径移动。
        :param speed: 调度机器人按照该速度序列依次在path上的各路段移动。len(speed) = len(path) - 1
        """
        if self._state == Robot.STATE_IDLE or self._state == Robot.STATE_PAUSE:
            self._state = Robot.STATE_GOING_TO_DST
            self._path = path
            self._speed = speed
            self._last_waypoint_index = 0 # 上一个waypoint的索引（此处从0开始）
            self._vel = self._calcDesiredVelocity(0)
        elif self._state == Robot.STATE_GOING_TO_DST:
            if self._path[self._last_waypoint_index] == path[0]:
                self._path = path
                self._speed = speed
                self._last_waypoint_index = 0 # 上一个waypoint的索引（此处从0开始）
                self._vel = self._calcDesiredVelocity(0)
            elif self._last_waypoint_index < len(self._path)-1 and \
                 self._path[self._last_waypoint_index+1] == path[0]:
                self._path = [self._path[self._last_waypoint_index]] + path
                self._speed = [self._speed[self._last_waypoint_index]] + speed
                self._last_waypoint_index = 0 # 上一个waypoint的索引（此处从0开始）
                self._vel = self._calcDesiredVelocity(0)
                
    def _pause(self):
        self._state = Robot.STATE_PAUSE

    def reportWaypoint(self, waypoint, delay):
        """
        向服务器上报抵达节点的信息。
        :param waypoint: 上报抵达的节点。
        """
        self._comm_channel.clientSendMessage(Message(self._timer.getTime(),
                                                     self._id,
                                                     None, # we don't need to specify the receiver, since there is only one server.
                                                     Message.CMD_ROBOT_POS_UPDATE, 
                                                     waypoint, 
                                                     delay=delay))

    def _calcStateVariables(self, dt, old_pos, old_vel, corner_waypoint_pos, new_vel):
        """
        计算运动的状态变量（位置、速度）。在初始时，机器人的位置和速度分别是old_pos、old_vel
        dt时间后，若机器人跨越了corner_waypoint速度有可能变为new_vel（当前仿真中，机器人是通过速度控制的，
        因此速度是一个外部输入量）。据此计算dt时间后的位置、速度。
        :param dt: 计算dt时间后的位置、速度。
        :param old_pos: 当前位置。
        :param old_vel: 当前速度。
        :param corner_waypoint_pos: corner_waypoint的位置。
        :param new_vel: 若跨越corner_waypoing后，新的速度。
        :return: dt时间后，新的位置、速度。
        """
        overshoot = old_pos + old_vel*dt-corner_waypoint_pos
        if old_vel.norm() > 1e-6 and overshoot.dot(old_vel)/old_vel.norm() > -1e-6: # overshoot
            return corner_waypoint_pos + new_vel*(overshoot.norm()/old_vel.norm()), new_vel
        else:
            return old_pos + old_vel*dt, old_vel

    def _calcDesiredVelocity(self, i):
        """
        计算self._path上第i条边上的速度。该速度是由调度结果(保存在self._path, self._speed)决定。
        :param i: 求self._path上第i条边上的速度。
        :return: self._path上第i条边上的速度
        """
        if i >= len(self._path) - 1:
            return Vector(0.0, 0.0)
            
        speed = self._speed[i]
        direction = self._map.getNodeCoord(self._path[i+1])-self._map.getNodeCoord(self._path[i])
        direction /= direction.norm()
        return direction * speed
                
    def step(self, dt):
        """
        仿真dt时间内，机器人的行为。
        :param dt: 步进dt时间。
        """
        msg = self._comm_channel.clientFetchMessage(self._id)
        if msg:
            if msg.getCommand() == Message.CMD_TCS_SCHEDULE:
                #print('Time', self._timer.getTime(), 'robot', self._id, 'received schedule message')
                path, speed = msg.getParam()
                self._schedule(path, speed)
            elif msg.getCommand() == Message.CMD_TCS_PAUSE:
                self._pause()
        
        if self._state == Robot.STATE_GOING_TO_DST:
            if self._obstacle_pool.obstacleExistInTheWay(self._pos, 
                                                         self._vel, 
                                                         self._detection_range, 
                                                         self._detection_angle):
                return
                
            #if self._robot_pool.robotExistInTheWay(self):
            #    return
            
            # desired velocity on the next edge
            if self._last_waypoint_index >= len(self._path) - 2:
                vel = Vector(0.0, 0.0)
            else:
                vel = self._calcDesiredVelocity(self._last_waypoint_index+1)
                
            corner_waypoint_pos = self._map.getNodeCoord(self._path[self._last_waypoint_index+1])
            self._pos, self._vel = self._calcStateVariables(dt, self._pos, self._vel, corner_waypoint_pos, vel)
            #print('Time', self._timer.getTime(), 'robot', self._id, 'updated position', self._pos.getX(), self._pos.getY())
            if self._vel is vel:
                if self._last_waypoint_index >= len(self._path) - 2:
                    self._state = Robot.STATE_REACH_DST
                    #print('Robot {0} reached destination node {1}'.format(self._id, self._path[-1]))
                    self._last_waypoint = self._path[-1]
                    self.reportWaypoint(self._path[-1], 0)
                    # FixMe: perform actions
                    self._state = Robot.STATE_IDLE
                    self._path = None
                    self._speed = None
                    self._last_waypoint_index = None
                else:
                    self._last_waypoint_index += 1
                    self._last_waypoint = self._path[self._last_waypoint_index]
                    self.reportWaypoint(self._path[self._last_waypoint_index], 0)

                
if __name__ == '__main__':
    pass
